from naoqi import ALModule
from naoqi import ALBroker
from naoqi import ALProxy
import math
import motion
import time
import threading
import doubleCatch
import sonar_detect6
import main
'''
global initangle
global speed_x
global speed_y
global speed_z
speed_x = 0.1
speed_z = 0.0
speed_y=0.0
NAO_IP = "192.168.1.102"
NAO_PORT = 9559
myBroker = ALBroker("myBroker",
                     "0.0.0.0",  # listen to anyone
                     0,  # find a free port and use it
                     NAO_IP,  # parent broker IP
                     NAO_PORT)  # parent broker port
memorypxy = ALProxy("ALMemory")
alproxy = ALProxy("ALMotion")
'''

class runthread(threading.Thread):
    def __init__(self,name):
        threading.Thread.__init__(self)
        self.name=name
    def run(self):
        while True:
            global speed_x,speed_y,speed_z
            alproxy.move(speed_x , speed_y , speed_z )
            time.sleep(0.1)
def processdata(data):

    global initangle
    if (data >= 0 and initangle <= 0):
        if (math.pi + initangle >= data):
            return -(data - initangle)
        else:
            return 2 * math.pi + initangle - data
    elif (data <= 0 and initangle >= 0):
        if (math.pi - initangle >= -data):
            return initangle - data
        else:
            return -(2 * math.pi - initangle + data)
    else:
        return initangle - data
def main1():

    global  speed_x
    global  speed_y
    global  speed_z

    time.sleep(3)
    speed_x=0.0
    speed_y=0.0
    speed_z=(math.pi * 2) / 15
    time.sleep(5)
    speed_z=0.0
    time.sleep(3)


    degree=memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    degree=processdata(degree)
    print degree
    needde=4*math.pi/3-degree
    speed_z=needde/5
    time.sleep(7)
    speed_z=0.0



if __name__=="__main__":
    #global initangle
    #initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    #run = runthread("B")
    #run.start();

    sonar_detect6.dete()
    #sonar_detect6.flag=1
    #global initangle
    #initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    #run = runthread("B")
    #run.start();
    #sonar_detect6.speed_x=0.0
    #sonar_detect6.speed_y=0.0
    #sonar_detect6.speed_z=(math.pi * 2) / 15
    #time.sleep(5)
    sonar_detect6.flag=1
    #movetoD
    main.findballo()
    doubleCatch.Catch()